#include "ros/ros.h"
#include "ros/console.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/LaserScan.h"
#include <std_msgs/String.h>
#include <std_msgs/Header.h>
#include "message_filters/subscriber.h"
#include "geometry_msgs/Pose2D.h"
#include <pcl_ros/point_cloud.h>

// pcl
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>

#include <Eigen/Dense>

#include <iostream>
#include "csm.h"
#include "egsl.h"
#include <fstream>

using namespace std;

typedef pcl::PointXYZ PointT;
// 使用PCL中点云的数据结构 pcl::PointCloud<pcl::PointXYZ>
typedef pcl::PointCloud<PointT> PointCloudT;

class icp
{
    public:
    icp();
    ~icp();
    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
    void laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& msg, LDP& laser);
    void CalcuNormalVector(const sensor_msgs::LaserScan::ConstPtr& msg);
    void laserScanToLDP(const sensor_msgs::LaserScan msg, LDP& laser);

    pcl::PointCloud<PointT> ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg);

    bool static inline distance_valid(double r, double min_r, double max_r)
    {
        if (r > min_r && r < max_r)
            return true;
        else
            return false;
    }

    double static inline distance(double p1_x, double p1_y, double p2_x, double p2_y)
    {
        return sqrt(pow(p1_x - p2_x, 2) + pow(p1_y - p2_y, 2));
    }

    private:

    double key_scan_threshold;
    double delta[3] = { 0.0 , 0.0 , 0.0 };
    double prev_delta[3] = { 0.0 , 0.0 , 0.0 };
    CSM* csm;
    double corres_ratio = 0.0;
    bool first;
    struct sm_params params;
    struct sm_result result;

    LDP laser_ref;
    LDP laser_incidence_angle;
    ros::Subscriber laser_scan_sub_;
    ros::NodeHandle nh_;
};

icp::icp(){
  
   csm = new CSM();
   
   first = true;

   // ************************ICP匹配相关参数设计********************************** 
   params.sigma = 0.01;//表征激光雷达测距噪声参数，单位m
   params.use_corr_tricks = 1;//ICP对应点查找是否启动高效查找策略开关项，1表示开，0表示关
   params.max_correspondence_dist =1.5;//ICP对应点查找时最近邻点间欧式距离值最大允许差值，单位m
   params.max_iterations = 10;//ICP迭代最大代数
   params.outliers_maxPerc = 0.9;//
   params.outliers_adaptive_mult = 2.0;
   params.outliers_adaptive_order = 0.85;
   params.outliers_remove_doubles = 1;
   params.use_point_to_line_distance = 1;//ICP目标函数数学模型，1表示点到线残差模型，0表示点到点残差模型，一般情况1模式下ICP计算效率更高，输出结果更好
   params.do_alpha_test = 0;
   params.do_compute_covariance = 1;//计算ICP匹配位姿协方差开关项，1表示开，0表示关
   params.use_ml_weights = 0;
   params.use_sigma_weights = 0;
   params.min_reading = 0.0;//激光数据测距范围
   params.max_reading = 30.0;
   params.laser[0] = 0.0;
   params.laser[1] = 0.0;
   params.laser[2] = 0.0;
   // *****************************************************************************
   
   // 定义回调函数，当话题名上有消息发布出来时触发该icp::laserCallback()回调函数功能
   laser_scan_sub_ = nh_.subscribe("/scan", 1 , &icp::laserCallback , this);
}

icp::~icp(){
    if(csm) delete csm;
}

// 实现相邻帧间激光数据的ICP匹配
void icp::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    if(!msg)
      return;
    
    ros::Time t1 = ros::Time::now();
    // 计算当前帧激光数据的法向量角度
    CalcuNormalVector(msg);
    ros::Time t2 = ros::Time::now();

    //如果当前激光帧是第一帧，则将当前激光帧数据赋值给laser_ref变量
    if(first)
    {
        laserScanToLDP(msg, laser_ref);
	
        first = false;
	
        return;
    }
    else
    {

        LDP laser_sens;
    	//当前帧激光数据赋值给laser_sens指针变量	
        laserScanToLDP(msg, laser_sens);
	
	// 判断两帧激光数据本身是否有效
        if(!ld_valid_fields(laser_ref)){
            printf("ScanToMap-Invalid laser data in first scan.\n");
            first = true;
            ld_free(laser_ref);
            return;
        }
        if(!ld_valid_fields(laser_sens)){
            printf("ScanToMap-Invalid laser data in second scan.\n");
            ld_free(laser_sens);
            return;
        }
	
	// 将连续两帧激光数据赋值给params参数变量
	params.laser_ref = laser_ref;
	
        params.laser_sens = laser_sens;
	
	// 两帧激光数据下激光扫描仪的初始相对位姿赋值给params参数变量
        params.first_guess[0] = delta[0];

        params.first_guess[1] = delta[1];

        params.first_guess[2] = delta[2];

        ros::Time t1 = ros::Time::now();
	// 执行两帧激光数据ICP解算
        csm->sm_icp(&params, &result, &corres_ratio);

        ros::Time t2 = ros::Time::now();

        //std::cout <<"icp cost time:"<< (t2-t1).toSec()<<",and corres ratio is:"<<corres_ratio<<std::endl;	
	
	// 判断ICP解算的结果是否有效，如有效则提取解算出的相对位姿
        if(!result.valid){
            printf("[icp][error] ICP result unvalid! ICP stop!\n");
            first = false;
            ld_free(laser_ref);
            ld_free(laser_sens);
            return;
         }
        else{
	  // 计算当前帧激光扫描仪的相对位姿,laser_sens相对于laser_ref的位姿
	  delta[0] = result.x[0];
	  delta[1] = result.x[1];
	  delta[2] = result.x[2];
	  delta[2] = atan2(sin(delta[2]), cos(delta[2]));
	  printf("ICP solve's delta pose is: (%9.4f[m], %9.4f[m], %9.4f[deg]).\n", delta[0], delta[1], delta[2] * 180 / M_PI);	

          // 释放laser_ref指针变量，并将当前帧激光帧数据赋值给laser_ref
	  ld_free(laser_ref);
	  laser_ref = laser_sens;  
        }
    }
}

//实现对原始一帧激光数据的预处理
void icp::laserScanToLDP(const sensor_msgs::LaserScan msg, LDP& laser)
{
    int step = 1;//一帧激光数据下采样倍数

    double delta_a = msg.angle_increment;

    laser = ld_alloc_new(msg.ranges.size()/step);

    laser->nrays = msg.ranges.size()/step;

    int num_rays = 0;
    
    for (int i = 0; i < msg.ranges.size(); i += step)
    {
        double r = msg.ranges[i];
        if ((r > msg.range_min) && (r < msg.range_max))
        {
            laser->readings[num_rays] = r;
            laser->valid[num_rays] = 1;
            laser->alpha[num_rays] = laser_incidence_angle->alpha[i];
        }else{
            laser->readings[num_rays] = 0;
            laser->valid[num_rays] = 0;
            laser->alpha[num_rays] = GSL_NAN;
        }
        laser->theta[num_rays] = (double)(msg.angle_min + i * delta_a);
        laser->cluster[num_rays] = -1;
        num_rays++;
    }

    laser->max_theta = laser->theta[num_rays - 1];
    laser->min_theta = laser->theta[0];
}

//实现对原始一帧激光数据的预处理
void icp::laserScanToLDP(const sensor_msgs::LaserScan::ConstPtr& msg, LDP& laser)
{
    int step = 1;//一帧激光数据下采样倍数

    double delta_a = msg->angle_increment;

    laser = ld_alloc_new(msg->ranges.size()/step);

    laser->nrays = msg->ranges.size()/step;

    int num_rays = 0;
    //laser->nrays = msg->ranges.size();
    for (int i = 0; i < msg->ranges.size(); i += step) 
    {
        double r = msg->ranges[i];
        if ((r > msg->range_min) && (r < msg->range_max))
        {
            laser->readings[num_rays] = r;
            laser->valid[num_rays] = 1;
            laser->alpha[num_rays] = laser_incidence_angle->alpha[i];
        }else{
            laser->readings[num_rays] = 0;
            laser->valid[num_rays] = 0;
            laser->alpha[num_rays] = GSL_NAN;
        }
        laser->theta[num_rays] = (double)(msg->angle_min + i * delta_a);
        laser->cluster[num_rays] = -1;
        num_rays++;
    }

    laser->max_theta = laser->theta[num_rays - 1];
    laser->min_theta = laser->theta[0];
}

//计算一帧激光数据中每束激光端点的法向量角度值
void icp::CalcuNormalVector(const sensor_msgs::LaserScan::ConstPtr& msg)
{
    laser_incidence_angle = ld_alloc_new(msg->ranges.size());
    laser_incidence_angle->nrays = msg->ranges.size();
    double delta_a = msg->angle_increment;

    double Roi_dist = msg->range_max;

    for(int i = 0; i < laser_incidence_angle->nrays; i++)
    {
        int index_down = i-1 < 0 ? (msg->ranges.size()-1):(i-1);
        int index_up = i+1 > (msg->ranges.size()-1) ? 0 : (i+1);
        double j = msg->ranges[i];
        double j_d = msg->ranges[index_down];
        double j_u = msg->ranges[index_up];
        laser_incidence_angle->theta[i] = msg->angle_min + i*msg->angle_increment;

        if(!distance_valid(j, msg->range_min, msg->range_max))//该束激光距离异常，相应地将其对应的法向量置为无效
        {
            laser_incidence_angle->alpha_valid[i] = 0;
            laser_incidence_angle->alpha[i] = GSL_NAN;
        }
        else//该激光束距离有效:待进一步判断
        {
            double threshold = 1.5 * j * delta_a;//相邻两束激光打到同一物体上的距离判断阈值
            laser_incidence_angle->readings[i] = j;

            if(!distance_valid(j_d, msg->range_min, msg->range_max) && 
               !distance_valid(j_u, msg->range_min, msg->range_max))//该激光束左右两侧均为无效点，那么其也无效
               {
                   laser_incidence_angle->alpha_valid[i] = 0;
                   laser_incidence_angle->alpha[i] = GSL_NAN;
               }
            else if(!distance_valid(j_d, msg->range_min, msg->range_max) &&
                     distance_valid(j_u, msg->range_min, msg->range_max))//DOWN点无效，UP点有效
                     {
                         double j_x = j * cos(laser_incidence_angle->theta[i]);
                         double j_y = j * sin(laser_incidence_angle->theta[i]);
                         double ju_x = j_u * cos(laser_incidence_angle->theta[i]+delta_a);
                         double ju_y = j_u * sin(laser_incidence_angle->theta[i]+delta_a);
                         double dist = distance(j_x, j_y, ju_x, ju_y);
                         
                         if(dist > threshold)
                         {
                             laser_incidence_angle->alpha_valid[i] = 0;
                             laser_incidence_angle->alpha[i] = GSL_NAN;
                         }
                         else
                         {
                             laser_incidence_angle->alpha_valid[i] = 1;
                             //注意，此处需要用UP点的坐标减去当前点(CP)的坐标，得到的向量是CP->UP向量
                             //再将上述向量与X轴的夹角逆时针旋转90度，得到该激光束的法向量与X轴的夹角;
                             //角度需要转换到[-PI,PI]
                             double incidenct_angle = atan2(ju_y-j_y, ju_x-j_x) + M_PI/2;
                             laser_incidence_angle->alpha[i] = atan2(sin(incidenct_angle), cos(incidenct_angle));
                         }
                     }
            else if(distance_valid(j_d, msg->range_min, msg->range_max) &&
                    !distance_valid(j_u, msg->range_min, msg->range_max))//DOWN点有效，UP点无效
                    {
                        double j_x = j * cos(laser_incidence_angle->theta[i]);
                        double j_y = j * sin(laser_incidence_angle->theta[i]);
                        double jd_x = j_d * cos(laser_incidence_angle->theta[i]-delta_a);
                        double jd_y = j_d * sin(laser_incidence_angle->theta[i]-delta_a);
                        double dist = distance(j_x, j_y, jd_x, jd_y);

                         if(dist > threshold)
                         {
                             laser_incidence_angle->alpha_valid[i] = 0;
                             laser_incidence_angle->alpha[i] = GSL_NAN;
                         }
                         else
                         {
                             laser_incidence_angle->alpha_valid[i] = 1;
                             //注意，此处需要用CP点的坐标减去DOWN的坐标，得到的向量是DOWN->CP向量
                             //再将上述向量与X轴的夹角逆时针旋转90度，得到该激光束的法向量与X轴的夹角;
                             //角度需要转换到[-PI,PI]
                             double incidenct_angle = atan2(j_y-jd_y, j_x-jd_x) + M_PI/2;
                             laser_incidence_angle->alpha[i] = atan2(sin(incidenct_angle), cos(incidenct_angle));
                         }
                    }

            else if(distance_valid(j_d, msg->range_min, msg->range_max) &&
                    distance_valid(j_u, msg->range_min, msg->range_max))//均有效
                    {
                        double j_x = j * cos(laser_incidence_angle->theta[i]);
                        double j_y = j * sin(laser_incidence_angle->theta[i]);
                        double jd_x = j_d * cos(laser_incidence_angle->theta[i] - delta_a);//此处有BUG～
                        double jd_y = j_d * sin(laser_incidence_angle->theta[i] - delta_a);
                        double ju_x = j_u * cos(laser_incidence_angle->theta[i] + delta_a);
                        double ju_y = j_u * sin(laser_incidence_angle->theta[i] + delta_a);

                        double dist2up = distance(j_x, j_y, ju_x, ju_y);
                        double dist2down = distance(j_x, j_y, jd_x, jd_y);

                        if(dist2up < threshold)
                        {
                            laser_incidence_angle->alpha_valid[i] = 1;
                            //注意，此处需要用UP点的坐标减去当前点(CP)的坐标，得到的向量是CP->UP向量
                            //再将上述向量与X轴的夹角逆时针旋转90度，得到该激光束的法向量与X轴的夹角;
                            //角度需要转换到[-PI,PI]
                            double incidenct_angle = atan2(ju_y-j_y, ju_x-j_x) + M_PI/2;
                            laser_incidence_angle->alpha[i] = atan2(sin(incidenct_angle), cos(incidenct_angle));
                        }
                        else if(dist2down < threshold)
                        {
                            laser_incidence_angle->alpha_valid[i] = 1;
                            //注意，此处需要用CP点的坐标减去DOWN的坐标，得到的向量是DOWN->CP向量
                            //再将上述向量与X轴的夹角逆时针旋转90度，得到该激光束的法向量与X轴的夹角;
                            //角度需要转换到[-PI,PI]
                            double incidenct_angle = atan2(j_y-jd_y, j_x-jd_x) + M_PI/2;
                            laser_incidence_angle->alpha[i] = atan2(sin(incidenct_angle), cos(incidenct_angle));
                        }
                        else
                        {
                            laser_incidence_angle->alpha_valid[i] = 0;
                            laser_incidence_angle->alpha[i] = GSL_NAN;
                        }
                    }
        }        
    }
}



pcl::PointCloud<PointT> icp::ConvertScan2PointCloud(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
    // PointCloudT::Ptr的数据类型为boost::shared_ptr
    PointCloudT::Ptr cloud_msg = boost::shared_ptr<PointCloudT>(new PointCloudT());
    // 对容器进行初始化
    cloud_msg->points.resize(scan_msg->ranges.size());

    for (unsigned int i = 0; i < scan_msg->ranges.size(); ++i)
    {
        // 首先声明一个 cloud_msg第i个点的 引用
        pcl::PointXYZ &point_tmp = cloud_msg->points[i];
        // 获取scan的第i个点的距离值
        float range = scan_msg->ranges[i];

        // 将 inf 与 nan 点 设置为无效点
        if (!std::isfinite(range))
            continue;

        // 有些雷达驱动会将无效点设置成 range_max+1
        // 所以要根据雷达的range_min与range_max进行有效值的判断
        if (range > scan_msg->range_min && range < scan_msg->range_max)
        {
            // 获取第i个点对应的角度
            float angle = scan_msg->angle_min + i * scan_msg->angle_increment;
            // 获取第i个点在笛卡尔坐标系下的坐标
            point_tmp.x = range * cos(angle);
            point_tmp.y = range * sin(angle);
            point_tmp.z = 0.0;
        }
    }

    // 高度为1的情况下, width即为所有点的个数
    cloud_msg->width = scan_msg->ranges.size();
    cloud_msg->height = 1;
    cloud_msg->is_dense = true; // not contains nans

    // 将scan_msg的消息头 赋值到 pcl::PointCloud<pcl::PointXYZ>的消息头
    pcl_conversions::toPCL(scan_msg->header, cloud_msg->header);


    return *cloud_msg;
    // 将转换完的数据赋值到current_pointcloud_中保存下来
    //*current_pointcloud_ = *filtered;
}



int main(int argc , char** argv) 
{
    ros::init(argc, argv, "lidarodom_demo");
    ros::NodeHandle nh;
    //实例化ICP类对象
    icp icp;
    ros::Rate r(50.0);
    while(nh.ok())
    {
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
